/*
 * besturing.c
 *
 *  Created on: 17 jun. 2011
 *      Author: Dirk-Jan
 */

#include <avr/io.h>

#include "besturing.h"
#include "regelaar.h"
#include "bluetooth.h"

void besturing_init(){
	blue_init();
	regelaar_init();
}


void besturing_task(void){
	static unsigned char receive_state, command, data;
	static unsigned int param;

	blue_task();

	if (bluetooth_data_ready()){
		data = bluetooth_get_data();
		switch (receive_state){
			case 0:
				command = data;
				receive_state++;
				break;
			case 1:
				param=0;
				param = ((unsigned char) data)<<8;
				receive_state++;
				break;
			case 2:
				param |= ((unsigned char) data);
				receive_state=255;
				break;
			case 3:		// commando's van de BT module
				 if (data==0x0A)
					receive_state = 0;
				break;
		}

		switch (command){
			case 0:	// gereserveerd, doet niks
				break;
			case 0x0A:// commando's van de BT module
				receive_state = 3;
				command=0;
				break;
			case 'a':		// accelero data opvragen
				bluetooth_send_char(get_accelero_data('x'));
				bluetooth_send_char(get_accelero_data('y'));
				bluetooth_send_char(get_accelero_data('z'));
				receive_state=0;
				break;
			case 'g':		// gyro data opvragen
				bluetooth_send_char(get_gyro_data('x'));
				bluetooth_send_char(get_gyro_data('y'));
				bluetooth_send_char(get_gyro_data('z'));
				receive_state=0;
				break;
			case 't':		// temperatuur data opvragen
				bluetooth_send_char(get_temp());
				receive_state=0;
				break;

			case 'd':		// draai offset
				if (receive_state>1){
					set_offset_draai(data);
					receive_state=0;
				}
				break;
			case 'l':		// lift offset
				if (receive_state>1){
					set_offset_lift(data);
					receive_state=0;
				}
				break;
			case 's':		// staart offset
				if (receive_state>1){
					set_offset_staart(data);
					receive_state=0;
				}
				break;

			case 'P':		// p-waarde voor regelaar instellen
				if (receive_state>2){
					set_P(param);
					receive_state=0;
				}
				break;
			case 'I':		// i-waarde voor regelaar instellen
				if (receive_state>2){
					set_I(param);
					receive_state=0;
				}
				break;
			case 'D':		// d-waarde voor regelaar instellen
				if (receive_state>2){
					set_D(param);
					receive_state=0;
				}
				break;
			case 'u':
				lock_motor_driver(0);
				break;

			case 'i':
				sensortest();
				receive_state=0;
				break;
			default:
				bluetooth_send_text("unknown command\n");
				receive_state=0;
				break;
		}
		if (receive_state==255) receive_state=0;

	}
}
